/*
 * Copyright (c) 2024 Black Sesame Technologies
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "dal_fastdds/dbh/DalDbhReader.hpp"
#include "dal_fastdds/utils/Uuid.hpp"
#include "dal_fastdds/listener_impl/DataReaderListenerImplDbhInfo.hpp"
#include "dal_fastdds/utils/log.hpp"
#include "dal_utils/MD5Helper.hpp"
#include "fastdds/dds/core/StackAllocatedSequence.hpp"
#include <fastdds/dds/subscriber/DataReader.hpp>
#include <fastdds/dds/subscriber/Subscriber.hpp>

DalDbhReader::DalDbhReader()
{
    const char* env_var_value = std::getenv("DAL_DBH_DEBUG");
    if (env_var_value != nullptr && std::atoi(env_var_value) == 1) {
        is_debug_print_enabled_ = true;
    }
}

DalDbhReader::~DalDbhReader()
{
    DmaBufHeapManager* h = DmaBufHeapManager::GetInstance();
    for (const auto& pair : block_map_) {
        auto block_ = pair.second;
        if (block_ && block_->global_fd != -1) {
            h->DestroyBlock(block_);
        }
    }

    fastdds_sub_->delete_datareader(dbh_send_notify_reader_);
    if (dbh_recv_notify_writer_)
        dbh_recv_notify_publisher_->delete_datawriter(dbh_recv_notify_writer_);
    if (dbh_recv_notify_publisher_)
        fastdds_participant_->delete_publisher(dbh_recv_notify_publisher_);
    fastdds_participant_->delete_topic(dbh_send_notify_topic_);
    fastdds_participant_->delete_topic(dbh_recv_notify_topic_);

    fastdds_sub_->delete_datareader(dbh_info_reader_);
    fastdds_participant_->delete_topic(dbh_info_topic_);
}

void DalDbhReader::SetFastddsSubscriber(Subscriber*& subscriber)
{
    fastdds_sub_ = subscriber;
}

void DalDbhReader::SetFastddsDomainParticipant(DomainParticipant*& participant)
{
    fastdds_participant_ = participant;
}

bool DalDbhReader::CreateDbhBuiltinEntities()
{
    return CreateDbhTopic() && CreateDbhSendNotifyReader() && CreateDbhRecvNotifyWriter() &&
           CreateDbhInfoReader();
}

extern "C" {
extern const dalidl_message_type_support_t*
dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapSendNotifyMsg();
extern const dalidl_message_type_support_t*
dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapRecvNotifyMsg();
extern const dalidl_message_type_support_t*
dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapInfo();
}

void DalDbhReader::SetDbhSendNotifyReaderListener(DataReaderListenerImpl* listener)
{
    dbh_send_notify_reader_listener_ = listener;
}

void DalDbhReader::SetDbhSendNotifyReaderQos(const DataReaderQos& qos)
{
    dbh_send_notify_reader_qos_ = qos;
}

void DalDbhReader::SetDbhSendNotifyReaderStatusMask(const StatusMask& mask)
{
    dbh_send_notify_reader_status_mask_ = mask;
}

void DalDbhReader::SetTypeSupport(const void* ts)
{
    if (!ts)
        return;
    msg_ts_callbacks_ = static_cast<const message_type_support_callbacks_t*>(ts);
    if (!msg_ts_) {
        msg_ts_ = new (std::nothrow) MessageTypeSupport_cpp(msg_ts_callbacks_);
    }
}

void DalDbhReader::SetOriginTopicName(const std::string& origin_topic_name)
{
    origin_topic_name_ = origin_topic_name;
}

bool DalDbhReader::CreateDbhTopic()
{
    if (origin_topic_name_.length() == 0 || origin_topic_name_ == "") {
        LOG_ERROR_WITH_LOCATION() << "origin topic name is empty" << std::endl;
        return false;
    }

    // !TODO more complex, use hash
    dbh_send_notify_topic_name_ = origin_topic_name_ + "_dbh_send_notify";
    dbh_recv_notify_topic_name_ = origin_topic_name_ + "_dbh_recv_notify";
    dbh_info_topic_name_ = origin_topic_name_ + "_dbh_info";

    // We don't load lib but call func directly.
    const dalidl_message_type_support_t* send_notify_ts_handle =
        dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapSendNotifyMsg();
    dbh_send_notify_msg_ts_callbacks_ =
        static_cast<const message_type_support_callbacks_t*>(send_notify_ts_handle->data);
    std::string send_notify_type_name = _create_type_name(dbh_send_notify_msg_ts_callbacks_);
    auto send_notify_tsupport =
        new (std::nothrow) MessageTypeSupport_cpp(dbh_send_notify_msg_ts_callbacks_);
    auto send_notify_ts = new TypeSupport(send_notify_tsupport);
    send_notify_ts->register_type(fastdds_participant_);
    dbh_send_notify_topic_ = fastdds_participant_->create_topic(
        dbh_send_notify_topic_name_, send_notify_type_name, TOPIC_QOS_DEFAULT);
    if (!dbh_send_notify_topic_)
        return false;

    // We don't load lib but call func directly.
    const dalidl_message_type_support_t* recv_notify_ts_handle =
        dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapRecvNotifyMsg();
    dbh_recv_notify_msg_ts_callbacks_ =
        static_cast<const message_type_support_callbacks_t*>(recv_notify_ts_handle->data);
    std::string recv_notify_type_name = _create_type_name(dbh_recv_notify_msg_ts_callbacks_);
    auto recv_notify_tsupport =
        new (std::nothrow) MessageTypeSupport_cpp(dbh_recv_notify_msg_ts_callbacks_);
    auto recv_notify_ts = new TypeSupport(recv_notify_tsupport);
    recv_notify_ts->register_type(fastdds_participant_);
    dbh_recv_notify_topic_ = fastdds_participant_->create_topic(
        dbh_recv_notify_topic_name_, recv_notify_type_name, TOPIC_QOS_DEFAULT);
    if (!dbh_recv_notify_topic_)
        return false;

    // We don't load lib but call func directly.
    const dalidl_message_type_support_t* info_ts_handle =
        dalidl_typesupport_fastrtps_cpp__get_message_type_support_handle__dal_builtin__msg__DalDmaBufHeapInfo();
    dbh_info_ts_callbacks_ =
        static_cast<const message_type_support_callbacks_t*>(info_ts_handle->data);
    std::string dbh_info_type_name = _create_type_name(dbh_info_ts_callbacks_);
    auto dbh_info_tsupport = new (std::nothrow) MessageTypeSupport_cpp(dbh_info_ts_callbacks_);
    auto dbh_info_ts = new TypeSupport(dbh_info_tsupport);
    dbh_info_ts->register_type(fastdds_participant_);
    dbh_info_topic_ = fastdds_participant_->create_topic(
        dbh_info_topic_name_, dbh_info_type_name, TOPIC_QOS_DEFAULT);
    if (!dbh_info_topic_)
        return false;

    return true;
}

DataReader* DalDbhReader::GetDbhSendNotifyReader()
{
    return dbh_send_notify_reader_;
}

bool DalDbhReader::RecvDbhSendNotifyMsg(eprosima::fastdds::dds::SampleInfoSeq& info_seq)
{
    dal_fastrtps_shared_cpp::SerializedData sdata;
    sdata.is_cdr_buffer = false;
    sdata.data = &dbh_send_notify_msg_;
    sdata.impl = dbh_send_notify_msg_ts_callbacks_;
    eprosima::fastdds::dds::StackAllocatedSequence<void*, 1> data_values;
    const_cast<void**>(data_values.buffer())[0] = &sdata;
    if (ReturnCode_t::RETCODE_OK != dbh_send_notify_reader_->take(data_values, info_seq, 1)) {
        return false;
    }

    // for debug
    if (is_debug_print_enabled_) {
        std::cout << "[dbh send notify reader <------]" << std::dec
                  << "  topic = " << dbh_send_notify_topic_name_
                  << ", global_fd = " << dbh_send_notify_msg_.global_fd
                  << ", mmap_size = " << dbh_send_notify_msg_.mmap_size
                  << ", msg_size = " << dbh_send_notify_msg_.msg_size
                  << ", index = " << dbh_send_notify_msg_.index << ", UUID = ";
        for (const auto& byte : dbh_send_notify_msg_.uuid) {
            std::cout << std::hex << +byte << " ";  // 将每个字节转换为十六进制表示并打印
        }
        std::cout << std::dec << std::endl;
    }

    return true;
}

void DalDbhReader::ParseDbhSendNotifyMsg()
{
    dbh_send_notify_global_fd = dbh_send_notify_msg_.global_fd;
    dbh_send_notify_offset = dbh_send_notify_msg_.offset;
    dbh_send_notify_mmap_size = dbh_send_notify_msg_.mmap_size;
    dbh_send_notify_msg_size = dbh_send_notify_msg_.msg_size;
    dbh_send_notify_index = dbh_send_notify_msg_.index;
    for (int i = 0; i < UUID_LEN; i++) {
        dbh_send_notify_uuid[i] = dbh_send_notify_msg_.uuid[i];
    }
}

bool DalDbhReader::GetMsgFromDbhBlock(void* msg)
{
    if (dbh_send_notify_global_fd == -1)
        return false;

    auto curr_block_ = GetDbhBlockFromMap(dbh_send_notify_global_fd);
    if (!curr_block_) {
        LOG_ERROR_WITH_LOCATION() << "GetDbhBlockFromMap failed" << std::endl;
        return false;
    }

    DmaBufHeapManager* h = DmaBufHeapManager::GetInstance();
    if (!h->SyncBlockReadMode(curr_block_)) {
        LOG_ERROR_WITH_LOCATION() << "SyncBlockReadMode failed" << std::endl;
        return false;
    }

    if (is_debug_print_enabled_) {
        std::cout << "[dbh send notify reader ------>]"
                  << ", md5sum = ";
        dal_utils::PrintMD5(reinterpret_cast<uint8_t*>(curr_block_->ptr), dbh_send_notify_msg_size);
    }

    eprosima::fastcdr::FastBuffer fastbuffer((char*)curr_block_->ptr, dbh_send_notify_msg_size);
    eprosima::fastcdr::Cdr deser(
        fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
    if (!msg_ts_->deserializeROSmessage(deser, msg, msg_ts_callbacks_)) {
        LOG_ERROR_WITH_LOCATION() << "deserializeROSmessage failed" << std::endl;
        return false;
    }

    return true;
}

bool DalDbhReader::CreateDbhInfoReader()
{
    DataReaderQos f_qos;
    f_qos.reliability().kind = RELIABLE_RELIABILITY_QOS;
    f_qos.history().kind = KEEP_ALL_HISTORY_QOS;
    f_qos.history().depth = 5;
    f_qos.durability().kind = VOLATILE_DURABILITY_QOS;
    // default is PREALLOCATED_MEMORY_MODE in fastrtps v2.6.1, but when sending string, we can't
    // judge it's length so must permit realloc.
    f_qos.endpoint().history_memory_policy =
        eprosima::fastrtps::rtps::MemoryManagementPolicy::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;

#ifdef DAL_FASTDDS_DATA_SHARING_DISABLED
    f_qos.data_sharing().off();
#endif

    auto dbh_info_listener = new DataReaderListenerImplDbhInfo(dbh_info_ts_callbacks_, this);
    dbh_info_reader_ = fastdds_sub_->create_datareader(
        dbh_info_topic_, f_qos, dbh_info_listener, StatusMask::data_available());
    if (dbh_info_reader_) {
        return true;
    }
    return false;
}

DmaBufHeapManager::Block* DalDbhReader::GetDbhBlockFromMap(int32_t global_fd)
{
    std::lock_guard<std::mutex> lkg(block_map_mutex_);

    DmaBufHeapManager::Block* curr_block_ = nullptr;
    DmaBufHeapManager* h = DmaBufHeapManager::GetInstance();

    if (block_map_.find(global_fd) != block_map_.end()) {
        curr_block_ = block_map_[global_fd];
        return curr_block_;
    }

    curr_block_ = new DmaBufHeapManager::Block();
    curr_block_->global_fd = global_fd;
    curr_block_->size = dbh_send_notify_mmap_size;
    curr_block_->ptr = nullptr;
    curr_block_->dmabuf_fd = -1;
    block_map_[global_fd] = curr_block_;

    if (!h->GetMappedBlock(curr_block_)) {
        LOG_ERROR_WITH_LOCATION() << "GetMappedBlock failed" << std::endl;
        delete curr_block_;
        curr_block_ = nullptr;
        return nullptr;
    }

    return curr_block_;
}

bool DalDbhReader::DeleteDbhBlockFromMap(int32_t global_fd)
{
    std::lock_guard<std::mutex> lkg(block_map_mutex_);

    if (block_map_.find(global_fd) != block_map_.end()) {
        auto curr_block_ = block_map_[global_fd];
        DmaBufHeapManager* h = DmaBufHeapManager::GetInstance();
        if (!h->DestroyBlock(curr_block_))
            return false;
        block_map_.erase(global_fd);
    }

    return true;
}
